#include "FollowWayPointsState.h"
#include "PDController.h"
#include "AttractiveField.h"
#include "RandomField.h"

FollowWayPointsState::FollowWayPointsState(vector <Vector2> & waypoints) {
	_waypoints = waypoints;
	_done = false;
	cur_waypoint = -1;
	setNextWayPoint();
}

void FollowWayPointsState::setNextWayPoint() {
	if (cur_waypoint < ((int)_waypoints.size() - 1)) {
		cur_waypoint++;
	}
	else {
		cur_waypoint = -1;
		_done = true;
	}
}


FollowWayPointsState::~FollowWayPointsState(void) {
}

Command FollowWayPointsState::GetCommand(GridAgent* agent) {
	if (cur_waypoint >= 0) {
		tank_t* tank = agent->getTank();
		Vector2 tank_pos(tank->pos[0], tank->pos[1]);
		Vector2 waypoint = _waypoints.at(cur_waypoint);

		double mag = (last_pos - tank_pos).magnitude();
		if (mag < 1) {
			/*
			_waypoints.push_back(tank_pos);
			_waypoints.back().x += rand() % 50 - 25;
			_waypoints.back().y += rand() % 50 - 25;

			for (unsigned int i = _waypoints.size() - 1; i > cur_waypoint; i--) {
				Vector2 temp = _waypoints.at(i);
				_waypoints.at(i - 1) = _waypoints.at(i);
				_waypoints.at(i) - temp;
			}
			*/
		}

		last_pos = tank_pos;

		AttractiveField field(waypoint);
		RandomField rand_field;
		Vector2 trajectory = field.get_trajectory(tank_pos);
		trajectory += rand_field.get_trajectory(tank_pos);

		Command cmd = PDController::instance()->GetComand(tank, trajectory);
		float dist = sqrt(pow(tank->pos[0] - waypoint.x, 2) + pow(tank->pos[1] - waypoint.y, 2));
		if (dist < 20) {
			setNextWayPoint();
		}

		return cmd;
	}
	else {
		return Command(agent->getTank()->index, 0, 0, false, false);
	}
}

bool FollowWayPointsState::done() {
	return _done;
}

bool FollowWayPointsState::complete() {
	return _done;
}
